#include "loc_correction/filter2.h"


MovingAverageFilter_quat::MovingAverageFilter_quat(int window_size) : window_size_(window_size) {}


Eigen::Quaterniond MovingAverageFilter_quat::apply(const Eigen::Quaterniond& new_value) {
    if (position_queue_.size() >= window_size_) {
        position_queue_.pop();
    }
    position_queue_.push(new_value);

    Eigen::Quaterniond average = position_queue_.front();
    int count = 1;
    std::queue<Eigen::Quaterniond> temp_queue = position_queue_;
    temp_queue.pop();  // 第一个已经赋值给了 average

    while (!temp_queue.empty()) {
        average = average.slerp(1.0 / (++count), temp_queue.front());
        temp_queue.pop();
    }

    return average;
}


